#include <arduino/Arduino.h>
#include <servo/sg90.h>


int sonar_input = 3;
int sonar_output = 2;
int led = 8;
servo::sg90* serv;


void setup(void)
{
  Serial.begin(9600);

  pinMode(led, OUTPUT);

  serv = new servo::sg90(6);

  pinMode(sonar_input , INPUT);
  pinMode(sonar_output, OUTPUT);
}


void blink(int time)
{
  digitalWrite(led, HIGH);
  delay(time);
  digitalWrite(led, LOW);
  delay(time);
}


int get_distance(void)
{
  // Send signal to sonar.
  digitalWrite(sonar_output, LOW);
  delayMicroseconds(2);
  digitalWrite(sonar_output, HIGH);
  delayMicroseconds(10);
  digitalWrite(sonar_output, LOW);

  // Wait for response.
  int duration = pulseIn(sonar_input, HIGH);
  return duration/58;
}


void loop(void)
{
  int distance = get_distance();
  blink(distance*2);
  serv->set_position(distance);
  delay(100);
}
